#ifndef GCSLINK_H
#define GCSLINK_H

#include <common/Runnable.h>
#include <common/Semaphore.h>
#define ORBSUBSCRIBER_WORKAROUND_NAMESPACE gcslink
#include <common/orbhelper.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/zorfcs_state.h>
#include <uORB/topics/zorfcs_request.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/engine.h>
#include <uORB/topics/battery_status.h>

class SerialPort;

class GcsLink : public Runnable {
public:
	GcsLink(SerialPort *port);
	virtual ~GcsLink();
	
private:
	GcsLink();
	virtual void run();
	void init();	
	void checkTimeout();
	void resetTimeout(uint8_t id);
	bool parsePacket(uint8_t id, uint8_t *data, int &size);
	void updateOrbData();

	typedef uint8_t (GcsLink::*HandlerPtr)(uint8_t *, uint8_t, uint8_t);
	HandlerPtr searchHandlerTable(uint8_t id, int size);

	uint8_t info(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t telemetry(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t hardwareState(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t powerModules(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t activeLeg(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t satellites(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t setpoints(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t manual(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t command(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t getPar(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t setPar(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t getWayPoint(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t setWayPoint(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t gotoWayPoint(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t setOperPoint(uint8_t *data, uint8_t size, uint8_t maxsize);	
	uint8_t setADSNullPoint(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t resetADS(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t gymbalControl(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t datalinkControl(uint8_t *data, uint8_t size, uint8_t maxsize);
	uint8_t group(uint8_t *data, uint8_t size, uint8_t maxsize);
	
	void sendZorFcsRequest(int type, void *data);

	SerialPort *port_;
	static const int buffersize_;
	uint8_t *buffer_;
	uint8_t *buffer2_;
	static const uint16_t timeOut_;
	uint16_t requestTime_;
	uint32_t writeCounter_;
	uint8_t selfAddr_;

	struct Handler {
		uint8_t id;
		int size;
		HandlerPtr handler;
	};

	const Handler *handlerTable_;
	int handlerTableSize_;

	orbhelper::OrbSubscriber<vehicle_attitude_s, ORB_ID(vehicle_attitude)> vehicle_attitude_;
	orbhelper::OrbSubscriber<vehicle_gps_position_s, ORB_ID(vehicle_gps_position)> vehicle_gps_position_;
	orbhelper::OrbSubscriber<vehicle_global_position_s, ORB_ID(vehicle_global_position)> vehicle_global_position_;
	orbhelper::OrbSubscriber<actuator_controls_s, ORB_ID_VEHICLE_ATTITUDE_CONTROLS> actuator_controls_;
	orbhelper::OrbSubscriber<zorfcs_state_s, ORB_ID(zorfcs_state)> zorfcs_state_;
	orbhelper::OrbSubscriber<engine_s, ORB_ID(engine)> engine_;
	orbhelper::OrbSubscriber<battery_status_s, ORB_ID(battery_status)> battery_status_;
	orbhelper::OrbPublisher<zorfcs_request_s, ORB_ID(zorfcs_request)> zorfcs_request_;
	orbhelper::OrbPublisher<manual_control_setpoint_s, ORB_ID(manual_control_setpoint)> manual_control_setpoint_;

	Semaphore sem_;
};

#endif
